package engine;
import java.util.*;

import quadTree.BoundingBox;
import quadTree.QuadNode;
public class PhysicsObject {

	//@invariant VertList.size()>0; 
	private /*@ spec_public @*/ int index;
	private /*@ spec_public @*/ List<Vertex> VertList;
	private /*@ spec_public @*/ List<Edge> EdgeList;
	private /*@ spec_public @*/ float mass;
	private /*@ spec_public @*/ Vector position;
	private /*@ spec_public @*/ Vector velocity;
	private /*@ spec_public @*/ float angularVelo;
	private /*@ spec_public @*/ float angle;
	private /*@ spec_public @*/ float inertiaTensor;
	private /*@ spec_public @*/ boolean isStatic;
	protected /*@ spec_public @*/ ArrayList<Integer> colliders;
	
	public ArrayList<Vector> temp_transform;
	/**
	 * Reference to the current QuadNode that 
	 * contains this PhysicsObject.
	 */
	public /*@ spec_public @*/ QuadNode parent; // NOTE: I would like this to be protected. To do that the
												// current package structure will need to be reorganized.
	//new stuff
	/**
	 * Min (Upper left corner) of a bounding box containing
	 * the physics object oriented around the origin.
	 */
	private /*@ spec_public @*/ Vector relativeMin;
	/**
	 * Max (Lower right corner) of a bounding box containing
	 * the physics object oriented around the origin.
	 */
	private /*@ spec_public @*/ Vector relativeMax;
	/**
	 * PhysicsObject Constructor
	 * @param index the index of the object in the Scene
	 * @param VertList the list of Vertices that make up the PhysicsObject
	 * @param mass the mass of the PhysicsObject
	 * @param edges the Edges between Vertices
	 * @param velocity The overall velocity of the object
	 */
	/*
	 * @assignable index, velocity, VertList, mass, velocity;
	 * @requires index >= 0;
	 * @requires VertList != null;
	 * @requires mass > 0;
	 * @requires edges != null;
	 * @requires velocity != null;
	 * @ensures this.index == index;
	 * @ensures this.mass == mass;
	 * @ensures this.VertList == VertList;
	 * @ensures this.velocity == velocity;
	 * @ensures this.EdgeList == edges;
	 */
	public PhysicsObject(int index, List<Vertex> VertList, float mass, List<Edge> edges, Vector velocity) {
		this.index = index;
		this.VertList = VertList;
		// getBoundingBox() will set the position the first time it is called this.position = this.getCenterOfMass();
		getBoundingBox();
		
		this.velocity = velocity;
		
		/*
		if(velocity==null)
		{
			velocity = new Vector(0,0);
			for(Vertex v:VertList)
			{
				velocity.setX((velocity.getX()+v.getVelo().getX()));
				velocity.setY((velocity.getY()+v.getVelo().getY()));
			}
				velocity.setX((velocity.getX()/((float)VertList.size())));
				velocity.setY((velocity.getY()/((float)VertList.size())));
		}*/
		EdgeList = edges;
		this.mass = mass;
		isStatic = false;
		angularVelo = 0;
		angle = 0;
		
		inertiaTensor = inertiaTensor();
		colliders = new ArrayList<Integer>();
		temp_transform = new ArrayList<Vector>();
	}
	/**
	 * getBoundingBox gets and returns a reference to the PhysicsObject's bounding box
	 * @return the PhysicsObject's bounding box
	 */
	/*
	 * @ requires VertList.size() > 0;
	 * @ ensures /result != null;
	 */
	public BoundingBox getBoundingBox(){
		
		// Execute all this crap only once.
		if(relativeMin == null) {
			Vector min, max, center;
			// nan so finding min and max is easier.
			min = new Vector(Float.NaN,Float.NaN);
			max = new Vector(Float.NaN,Float.NaN);
			
			// find the center of mass.
			center = position = getCenterOfMass();
			
			for(Vertex v : VertList){
					// check to see if v.x is smaller that the current min
				if(v.getPos().getX() < min.getX() || Float.isNaN(min.getX()))
					min.setX(v.getPos().getX());
					// check to see if v.x is larger that the current max
				if(v.getPos().getX() > max.getX() || Float.isNaN(max.getX()))
					max.setX(v.getPos().getX());
			
					// check to see if v.y is smaller that the current min
				if(v.getPos().getY() < min.getY() || Float.isNaN(min.getY()))
					min.setY(v.getPos().getY());
					// check to see if v.y is larger that the current min
				if(v.getPos().getY() > max.getY() || Float.isNaN(max.getY()))
					max.setY(v.getPos().getY());
			}
			
			// Sub from the center of mass so that all the points
			// are based around the origin
			//min.add(new Vector(-center.getX(),-center.getY()));
			//max.add(new Vector(-center.getX(),-center.getY()));
			
			relativeMin = new Vector(min.getX(), min.getY());
			relativeMax = new Vector(max.getX(), max.getY());
		
		}
		//Add to the current position to find the
		//bounding box in world space.
		Vector min = Vector.add(relativeMin, position);
		Vector max = Vector.add(relativeMax, position);
		
		return new BoundingBox(min, max);
	}

	/**
	 * isColliding takes another physics object and checks its edges
	 * against the edges of this physics object and will return a collision
	 * object if there are any intersect points
	 * @param o The physics object to compare with
	 * @return a collision object or null if there is no colliIntegersion
	 */
	/*
	 * @requires o != null;
	 */
	public Collision isColliding(PhysicsObject o){
		Matrix2X2 t0 = new Matrix2X2(angle);
		t0.setTranslation(position);
		
		Matrix2X2 t1 = new Matrix2X2(o.angle);
		t1.setTranslation(o.position);
		
		for(Edge e : EdgeList){
			Edge et = new Edge(new Vertex(t0.applyTransform(e.getV1().getPos())),
							   new Vertex(t0.applyTransform(e.getV2().getPos())),
							   0);
			this.temp_transform.add(et.getV1().getPos());
			
			for(Edge f : o.getEdges()){
				Edge ft = new Edge(new Vertex(t1.applyTransform(f.getV1().getPos())),
						   		   new Vertex(t1.applyTransform(f.getV2().getPos())),
						   		   0);
				
				Vector inter = et.intersectPoint(ft);
				
				if(inter!=null){
					Collision c = new Collision(this, o);
					
					
					c.setNormals(Vector.sub(position, inter), Vector.sub(o.getPosition(), inter));
					c.setImpact(inter);
					o.colliders.add(this.index);
					colliders.add(o.getIndex());
					
					return c;
				}
			}
		}
		return null;
	}
	
	/**
	 * Gets the vector describing the linear force of this object
	 * @return force vector
	 */
	/*
	 * @ ensures /result != null;
	 */
	protected Vector getLinForceDir(Vector contactPnt){
		//float f = this.velocity.magnitude() * this.mass;
		Vector force = Vector.mul(velocity, new Vector(mass,mass));//Vector.sub(contactPnt, position);
		//force.normalize();
		//force.scl(f);
		
		return force;
	}
	
	/**
	 * Gets the vector describing the rotational force of this object
	 * @param contactPnt - point of intersection between objects in world coordinates
	 * @return force vector
	 */
	/*
	 * @ requires contactPnt != null;
	 * @ ensures /result != null;
	 */
	protected Vector getRotForceDir(Vector contactPnt){
		Vector r = Vector.sub(position, contactPnt); // get vector for contact point
		//r.normalize();
		Vector forceDir = new Vector(r.getY() * this.angularVelo,
									 -r.getX() * this.angularVelo);
		forceDir.scl(this.mass);
		
		return forceDir;
	}
	
	/**
	 * getCenterOfMass finds the center of mass for the PhysicsObject and returns it as a vector in XY coordinate space
	 * @return the location of the PhysicsObject's center of mass
	 */
	/*
	 * @ requires VertList.size() > 0;
	 * @ ensures /result != null;
	 */
	public Vector getCenterOfMass(){
		Vector center = new Vector(0f, 0f);
		
		for(Vertex v : VertList){
			center.add(v.getPos());
		}
		
		center.scl(1f/VertList.size());
		
		for(Vertex v : VertList){
			v.getPos().sub(center);
		}
		
		return center;
	}			
	/**
	 * setMass sets the mass of the PhysicsObject to the passed parameter
	 * @param mass the mass this PhysicsObject's mass will be set to
	 */
	/*
	 * @ assignable mass;
	 * @requires mass > 0;
	 * @ensures this.mass == mass;
	 */	 
	public void setMass(float mass)
	{
		this.mass = mass;
	}
	/**
	 * setVelocity sets the velocity of the PhysicsObject to the passed Vector, absolute, not relative
	 * @param v the velocity the PhysicsObject will have after this method executes
	 */
	/*
	 * @ assignable velocity;
	 * @requires v != null;
	 * @ensures this.velocity == v;
	 */
	public void setVelocity(Vector v)
	{
		this.velocity = v;
	}
	
	/**
	 * setEdges sets the list of edges to the given input
	 * @param edges the edges that will replace the current list of edges
	 */
	/*
	 * @ assignable EdgeList;
	 * @requires edges != null;
	 * @ensures EdgeList == edges;
	 */
	public void setEdges(List<Edge> edges)
	{
		this.EdgeList = edges;
	}
	
	
	/**
	 * getEdges returns the edge list of this physics object to the caller
	 * @return EdgeList the edge list
	 */
	/*
	 * @requires this!=null;
	 * @returns List<Edge>;
	 */
	public List<Edge> getEdges()
	{
		return EdgeList;
	}
	/**
	 * update using time t, calculate the new position and velocity of the PhysicsObject
	 * @param t the time, in steps to update the object
	 * @param deg the amount to degrade velocity and angular velocity over time.
	 * @param netForce the net global force acting on the object's center of mass
	 * @param writeToLog indicates if we will generate a string for the logfile or not.
	 * @return A string logging the changes to the object if writeToLog is true, otherwise return null
	 */
	/*@ requires t>=0;
	 *@ requires deg>0;
	 *@ requires deg<=1;
	 *@ requires netForce != null;
	 *@ assignable ret;
	 *@ returns ret;
	 */
	public String update(float t, float deg, Vector netForce,boolean writeToLog)
	{
		colliders.clear();
		
		String ret = "";
		if (writeToLog) {
			ret += "\nBefore: " + this.toString();
			update(t, deg, netForce);
			ret += "\nAfter: " + this.toString();
		}
		else {
			update(t, deg, netForce);
			return ret;
		}
		return ret;
	}
	
	/**
	 * update using time t, calculate the new position, velocity, angle and angular velocity of the PhysicsObject
	 * @param t the time, in steps to update the object
	 * @param deg the percentage to degrade velocity and angular velocity over time
	 */
	/*@ requires t>=0;
	 *@ requires deg>0;
	 *@ requires deg<=1;
	 *@ assignable temp;
	 *@ assignable position;
	 *@ assignable velocity;
	 *@ assignable angle;
	 *@ assignable angularVelo;  
	 *@ returns /nothing
	 */
	public void update(float t, float deg)
	{ 
		colliders.clear();
		
		Vector temp = this.getVelocity();
		temp.scl(t);
		position.add(temp);
		velocity.scl(deg);
		angularVelo = angularVelo * deg * t;
		angle += angularVelo;
	}
	
	/**
	 * update using time and a force, change the PhysicsObject's position and Velocity
	 * @param t the time in steps to update the object
	 * @param deg the amount to degrade velocity and angular velocity with respect to time
	 * @param force the netforce to apply to the velocity 
	 */
	/*@ requires t>=0;
	  @ assignable position;
	  @ assignable velocity;
	 */
	public void update(float t, float deg, Vector force)
	{
		if(colliders.size()>0)
			colliders.clear();
		
		if(!isStatic){
			float dt = (t / 16f);
			
			Vector temp = force;
			temp.scl(1f/mass);
			velocity.add(temp);
			
			//temp = this.getVelocity();
			//temp.scl(dt);
			//this.getVelocity().scl(deg);
			
			position.add(this.getVelocity());
			velocity.scl(1f - ((1f - deg) * dt));
			angularVelo *= 1f - ((1f - deg) * dt);
			angle += angularVelo * dt;
		}
		else{
			velocity = Vector.Zero;
			angularVelo = 0;
		}
	}
	
	/**
	 * applyForces adds the effects of a force to the PhysicsObject
	 * @param f the force which will modify the PhysicsObject
	 * @param time the change in time for calculating how the force should be applied
	 * @return The string logging the changes on the object
	 */
	/*//Apply the local Force to the object
	 * @requires f != null;
	 * @requires t>0;
	 * @returns ret;
	 */
	public String applyForce(Force f, float time) {
		
		Vector direction = f.getDir();
		Vector accel = new Vector(0,0);
		accel.setX(direction.getX()/this.mass);
		accel.setY(direction.getY()/this.mass);
		String ret = "";
		ret += "Before applying force " + f + " " + this.toString() + "\n";
		position = PhysicsFunctions.position(position, velocity, time, accel);
		ret += "After applying force " + f + " " + this.toString() + "\n";
		return ret;

	}
	
	/**
	 * Applies a force f, at a specific point of the object
	 * the application of the force modifies the velocity accordingly.
	 * @param f Force to be applied
	 * @param contactPnt Point at which the force is applied.
	 */
	/*
	 * requires f != null;
	 * requires contactPnt != null;
	 */
	public void applyForce(Vector f, Vector contactPnt) {
		Vector r = Vector.sub(position, contactPnt);
		float invMass = 1f / this.mass;
		
		// modify the linear velVertListocity by applying the force f to this object
		//velocity.add(Vector.mul(f, new Vector(invMass, invMass)));
		
		float l = r.cross(f);	// angular momentum
		
		// modify the linear velVertListocity by applying the force f to this object
		velocity.add(Vector.mul(f, new Vector(invMass, invMass)));
		
		this.angularVelo -= (1f/inertiaTensor) * l;
		
		// going to try omiting eq. 6 in from GPU gems article.
		
	}
	
	/**
	 * getMass returns the mass of the object in 
	 * @return the PhysicsObject's mass
	 */
	/*
	 * @ assignable \nothing;
	 * @ returns mass;
	 * @ensures \result == mass;
	 */
	public float getMass()
	{
		return mass;
	}
	/**
	 * getVelocity returns the 2-D vector representing the PhysicsObject's velocity
	 * @return the PhysicObject's velocity
	 */
	/*
	 * @ assignable \nothing;
	 * @ returns velocity;
	 * @ensures \result == velocity;
	 */
	public Vector getVelocity()
	{
		return velocity;
	}

	/**
	 * getPosition returns the 2-D vector representing the PhysicsObject's position
	 * @return the PhysicObject's position
	 */
	/*
	 * @ assignable \nothing;
	 * @ returns position;
	 * @ensures \result == position;
	 */
	public Vector getPosition()
	{
		return position;
	}

	/**
	 * getVertexList returns the list of vertices used to make up the PhysicsObject
	 * @return the list of vertices
	 */
	/*
	 * @ assignable \nothing;
	 * @ returns VertList;
	 * @ ensures \result == VertList
	 */
	public List<Vertex> getVertexList()
	{
		return VertList;
	}
	
	/**
	 * Gets the index of the PhysicsObject in the Scene
	 * @return the PhysicsObject's index
	 */
	/*
	 * @assignable \nothing
	 * @returns index
	 */
	public int getIndex()
	{
		return index;
	}
	
	/**
	 * sets the index of the PhysicsObject in the Scene
	 * @return \nothing
	 */
	/*
	 * @assignable index
	 * @returns \nothing
	 */
	public void setIndex(int i)
	{
		index = i;
	}
	
	/**
	 * returns a String representation of the PhysicsObject
	 */
	/*
	 * @ assignable \nothing;
	 * @ returns new String;
	 */
	public String toString()
	{
		return "Physics Object: Index "+index+" Mass "+mass+" Velocity "+velocity+" Position "+position+ " Angle "+angle+" AngVelo "+angularVelo+" Inertia " +inertiaTensor+"\n"+"Edges: " + EdgeList;
	}
	
	/**
	 * Computes the inertiaTensor I of a physics object
	 * @return the value of the inertia tensor
	 */
	/*
	 * @ assignable \nothing;
	 * @returns float;
	 */
	public float inertiaTensor()
	{
		float r = 0f;
		
		for(Vertex v : VertList) {
			r += Vector.dot(v.getPos(), v.getPos());
		}
		return r * mass;
		
		/*float numerator   = 0;
		float denominator = 0;
		
		for(int i = 0; i < VertList.size(); i++){
			Vector p  = VertList.get(i).getPos();
			Vector p1 = VertList.get((i+1)%VertList.size()).getPos();
			
			float px = p.cross(p1);
			Vector pn1 = Vector.add(Vector.mul(p1, p1), p1);
			Vector pn  = Vector.add(Vector.mul(p, p), p);
			
			numerator   += px * (pn1.dot(pn));
			denominator += px;
		}
		
		return (mass/6) * (numerator/denominator);*/
	}
	/**
	 * getAngularVelo returns the angular velocity of the PhysicsObject
	 * @return the angular velocity
	 */
	/*
	 * @assignable /nothing;
	 * @returns float;
	 */
	public float getAngularVelo() {
		return angularVelo;
	}
	/**
	 * setAngularVelocity sets the PhysicsObject's angular velocity to the passed parameter
	 * @param angularVelo the PhysicsObject's new angular velocity
	 */
	/*
	 * @assignable this.angularVelo;
	 * @returns \nothing;
	 */
	public void setAngularVelo(float angularVelo) {
		this.angularVelo = angularVelo;
	}
	/**
	 * getAngle returns the angle by which the PhysicsObject has been rotated
	 * @return the angle of rotation
	 */
	/*
	 * @assignable \nothing;
	 * @returns float;
	 */
	public float getAngle() {
		return angle;
	}
	/**
	 * setAngle sets the angle by which the PhysicsObject has been rotated
	 * @param angle the angle by which the PhysicsObject will be rotated
	 */
	/*
	 * @assignable this.angle;
	 * @returns \nothing;
	 */
	public void setAngle(float angle) {
		this.angle = angle;
	}
	/**
	 * getInertiaTensor gets the value of the inertia tensor
	 * @return the value of the inertia tensor
	 */
	/*
	 * @assignable \nothing;
	 * @returns float;
	 */
	public float getInertiaTensor() {
		return inertiaTensor;
	}
	/**
	 * setInertiaTensor sets the value of the inertia tensor
	 * @param inertiaTensor the new value for the inertia tensor
	 */
	/*
	 * @assignable this.inertiaTensor;
	 * @returns \nothing;
	 */
	public void setInertiaTensor(float inertiaTensor) {
		this.inertiaTensor = inertiaTensor;
	}
	
	/**
	 * Set the flag that determines weather this object is static.
	 * @param isStatic
	 */
	/*
	 * @assignable this.isStatic;
	 * @returns \nothing;
	 */
	public void setStatic(boolean isStatic){
		this.isStatic = isStatic;
	}
}
